
import pybullet as p
import pybullet_data
from time import sleep



world_id = p.connect(p.GUI)
p.setGravity(0,0,-10)

p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane_id = p.loadURDF("plane.urdf")
robot_id = p.loadURDF("franka_panda/panda.urdf", [0,0,0])

p.createConstraint(robot_id,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])



endfetch_joint = 11
targetpos = [0.5,0.8,0.1]
joints_pose = p.calculateInverseKinematics(robot_id, endfetch_joint, targetPosition=targetpos)
try:
    
    while True:
        for i, pos in enumerate(joints_pose):
            p.setJointMotorControl2(robot_id, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=pos)
        # p.setJointMotorControl2(bodyUniqueId=robot_id, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=100)
        # p.setJointMotorControl2(bodyUniqueId=robot_id, jointIndex=1, controlMode=p.POSITION_CONTROL, targetPosition=1)
        p.stepSimulation()
        sleep(1/100)
except KeyboardInterrupt:
    pass
finally:
    p.disconnect()
    
    